#define deltaLimit 0.0001

double readShort(){

  int val1 = mySerial.read();
  int val2 = mySerial.read();
  return double( (val2 << 8) | val1 ) / 32768.00;
}


/*
//Serial handling functions

float readData(float oldValue){
  int packSize = 0;
  float tempData = 0;
  delay(5);
  packSize = Serial.read()-48;//Second character, size of data array
  tempData = 0;
  int tempChar = 0;
  for (int i = 0; i < packSize; i++){
    tempChar = Serial.read()-48;    
    tempData += pow(10, packSize - i - 1) * (tempChar); // Read data
    delay(5);
  }

  if(Serial.read() != '!') tempData = oldValue;
  return tempData;
}
*/
/*
float requestIMU(char type, int oldValue){
 mySerial.print(type);   
 unsigned long endTime = millis() + 100;
 while( true ){
 if(  millis() > endTime ) //Timeout, took too long
 return oldValue;
 if( mySerial.available() >= 2 ) //We have the number of avaliable bits, read it!
 break;
 }
 return readShort();
 } */

void printfromPC(){
  
    
  
  
  
  
  
}





/* =============================
  IMU Serial Reading / Printing
================================ */

float imuRowPrev = 0;
float imuPitPrev = 0;
float imuYawPrev = 0;
int imuRowRaw = 0;
int imuPitRaw = 0;
int imuYawRaw = 0;


//Transform the byte value read to a value from -1 to 1
float readIMUValue(int readVal){
  return float(readVal * 2) / 256.0 - 1.0;
}

void requestIMU(){
  
  imuRowPrev = imuRow;
  imuPitPrev = imuPit;
  imuYawPrev = imuYaw;

  if( mySerial.available() > 3 ){
    mySerial.flush();
    return;
  }

  unsigned long endTime = millis() + 5;
  while( millis() < endTime ){
    if( mySerial.available() >= 3 ){ //We have the number of avaliable bits, read it!
      imuRowRaw = mySerial.read();
      imuPitRaw = mySerial.read();
      imuYawRaw = mySerial.read();
      int checkSum = mySerial.read();
      
      if( (imuRowRaw+imuPitRaw+imuYawRaw) & 0xFF != checkSum)
        return;
    
      imuRow = readIMUValue( imuRowRaw );
      imuPit = readIMUValue( imuPitRaw );
      imuYaw = readIMUValue( imuYawRaw );
      
      //smoothing before returning
      //smoothIMU();
      return;
    }
  }
}

void smoothIMU(){
  if(abs(imuRow - imuRowPrev) > deltaLimit){
    if(imuRowPrev < imuRow)imuRow = imuRowPrev + deltaLimit;
    if(imuRowPrev > imuRow)imuRow = imuRowPrev - deltaLimit;
  }
  constrain(imuPit,(imuPitPrev-deltaLimit),(imuPitPrev+deltaLimit));
}





